#include <stdio.h>
#include <math.h>
#include "Steppermotor.h"

uint16_t Motor1TimeTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor1StepTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor2TimeTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor2StepTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor3TimeTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor3StepTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor4TimeTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};
uint16_t Motor4StepTable[2*(STEP_AA+STEP_UA+STEP_RA)+1]={0};

MOTOR_CONTROL_S motor1={0};
MOTOR_CONTROL_S motor2={0};
MOTOR_CONTROL_S motor3={0};
MOTOR_CONTROL_S motor4={0};

__IO uint8_t  MotionStatus = 0;                   // 是否在运动？0：停止，1：运动
__IO int32_t  Step_Position[2] = {0} ;            // 当前位置  单位:脉冲数
__IO int32_t  Rel_Position[2] = {0} ;             // 相对位置  单位:脉冲数
__IO uint32_t Toggle_Pulse = 1000;                // 脉宽

Interpolation_Typedef Arc={0};	//直线插补参数值

  /* 步进电机控制引脚,需要修改直接在stepmotor.h头文件修改即可*/
const StepMotor_CtrlTypedef Stepmotor[2]={\
{0, TIM_Channel_1,0,0,0,0},
{0, TIM_Channel_2,0,0,0,0},
};

void MotorRunParaInitial(void)
{ 
	/*FIXME:用户可以改变该参数实现S型曲线的升降特性*/ 
	CalcMotorPeriStep_CPF(M_FRE_START,M_FRE_AA,M_T_AA,M_T_UA,M_T_RA,Motor1TimeTable,Motor1StepTable); 
  // CalcMotorPeriStep_CPF(M_FRE_START,M_FRE_AA,M_T_AA,M_T_UA,M_T_RA,Motor2TimeTable,Motor2StepTable); 	
	// CalcMotorPeriStep_CPF(M_FRE_START,M_FRE_AA,M_T_AA,M_T_UA,M_T_RA,Motor3TimeTable,Motor3StepTable);
  // CalcMotorPeriStep_CPF(M_FRE_START,M_FRE_AA,M_T_AA,M_T_UA,M_T_RA,Motor4TimeTable,Motor4StepTable);	
}

 /*计算S型曲线算法的每一步定时器周期及步进数*/
void CalcMotorPeriStep_CPF(float fstart,float faa,float taa,float tua,float tra,uint16_t MotorTimeTable[],uint16_t MotorStepTable[])
{
  int  i;
	float fi;
	
	for(i=0;i<STEP_AA;i++){
		fi=GetFreAtTime(fstart,faa,taa,tua,tra,taa/STEP_AA*i);
		MotorTimeTable[i]=F2TIME_PARA/fi;
		MotorStepTable[i]=fi*(taa/STEP_AA)/STEP_PARA;
	}
	for(i=STEP_AA;i<STEP_AA+STEP_UA;i++){
		fi=GetFreAtTime(fstart,faa,taa,tua,tra,taa+(tua/STEP_UA)*(i-STEP_AA));
		MotorTimeTable[i]=F2TIME_PARA/fi;
		MotorStepTable[i]=fi*(tua/STEP_UA)/STEP_PARA;
	}
	for(i=STEP_AA+STEP_UA;i<STEP_AA+STEP_UA+STEP_RA;i++){
		fi=GetFreAtTime(fstart,faa,taa,tua,tra,taa+tua+tra/STEP_RA*(i-STEP_AA-STEP_UA));
		MotorTimeTable[i]=F2TIME_PARA/fi;
		MotorStepTable[i]=fi*(tra/STEP_RA)/STEP_PARA;
	}
	fi=GetFreAtTime(fstart,faa,taa,tua,tra,taa+tua+tra);
	MotorTimeTable[STEP_AA+STEP_UA+STEP_RA]=F2TIME_PARA/fi;
	MotorStepTable[STEP_AA+STEP_UA+STEP_RA]=fi*(tra/STEP_RA)/STEP_PARA;

	for(i=STEP_AA+STEP_UA+STEP_RA+1;i<2*(STEP_AA+STEP_UA+STEP_RA)+1;i++){ 
		MotorTimeTable[i]=MotorTimeTable[2*(STEP_AA+STEP_UA+STEP_RA)-i];
		MotorStepTable[i]=MotorStepTable[2*(STEP_AA+STEP_UA+STEP_RA)-i];
	}
}

/*根据S型曲线参数获取某个时刻的频率*/
float GetFreAtTime(float fstart,float faa,float taa,float tua,float tra,float t)
{
		//根据公式计算从开始到最高速过冲中，t时刻的转动频率
	  if(t>=0&&t<=taa){
			//加加速阶段
			return (fstart+0.5*faa*t*t);
		}else if(taa<t&&t<=(taa+tua)){
			//匀加速阶段
			return fstart+0.5*faa*taa*taa+(t-taa)*faa*taa;
		}else if((taa+tua)<t&&t<=(taa+tua+tra)){
			//减加速阶段
			return (fstart+0.5*faa*taa*taa+(tua)*faa*taa+0.5*faa*taa*tra-0.5*faa*taa*(taa+tua+tra-t)*(taa+tua+tra-t)/(tra));
		}		
		return 0;
}

 /**************************************************************************************
 初始化电机的参数，主要是细分选择，使用的定时器，顺时针方向值，电机ID等
 **************************************************************************************/
void Initial_Motor(unsigned char MotorID, unsigned char StepDive,unsigned int maxposition)
{
  unsigned int i=0;
	MOTOR_CONTROL_S *pmotor=NULL;
	uint16_t *MotorTimeTable=NULL;
	uint16_t *MotorStepTable=NULL;
	switch(StepDive){
    case 1:
      i=0x00;
      break;
    case 2:
      i=0x01;
      break;
    case 4:
      i=0x02;
      break;
    case 8:
      i=0x03;
      break;
    case 16:
      i=0x04;
      break;
    case 32:
      i=0x05;
      break;
    case 64:
      i=0x06;
      break;
    case 128:
      i=0x07;
      break;
    default:
      i=0x00;
      break;
	}
  switch(MotorID){
	  case 1:			 
			if(i&0x01){     //设置电机细分
			  // GPIO_SetBits(GPIOA,GPIO_Pin_11);
			}
			if(i&0x02){
			  // GPIO_SetBits(GPIOA,GPIO_Pin_12);
			}
			if(i&0x04){
				// GPIO_SetBits(GPIOE,GPIO_Pin_7);
			}
			// GPIO_SetBits(GPIOE,GPIO_Pin_8); //使能电机
			pmotor=&motor1;
			motor1.id=1;
			motor1.clockwise=M1_CLOCKWISE;
			motor1.TIMx=TIM3;
			MotorTimeTable=Motor1TimeTable;
			MotorStepTable=Motor1StepTable;			
			break;
		case 2:			 
			if(i&0x01){     //设置电机细分
			  // GPIO_SetBits(GPIOC,GPIO_Pin_0);
			}
			if(i&0x02){
			  // GPIO_SetBits(GPIOC,GPIO_Pin_1);
			}
			if(i&0x04){
				// GPIO_SetBits(GPIOC,GPIO_Pin_2);
			} 			
			// GPIO_SetBits(GPIOC,GPIO_Pin_3);  //使能电机
			pmotor=&motor2;
			motor2.id=2;
			motor2.clockwise=M2_CLOCKWISE;
			motor2.TIMx=TIM2;
			MotorTimeTable=Motor2TimeTable;
			MotorStepTable=Motor2StepTable;
			 break;
		case 3:
			if(i&0x01){     //设置电机细分
			  // GPIO_SetBits(GPIOA,GPIO_Pin_3);
			}
			if(i&0x02){
			  // GPIO_SetBits(GPIOA,GPIO_Pin_4);
			}
			if(i&0x04){
				// GPIO_SetBits(GPIOA,GPIO_Pin_5);
			}
			// GPIO_SetBits(GPIOC,GPIO_Pin_4); //使能电机
			pmotor=&motor3;
			motor3.id=3;
			motor3.clockwise=M3_CLOCKWISE;
			motor3.TIMx=TIM3;
			MotorTimeTable=Motor3TimeTable;
			MotorStepTable=Motor3StepTable;
			break;
		case 4:
      if(i&0x01){     //设置电机细分
			  // GPIO_SetBits(GPIOA,GPIO_Pin_3);
			}
			if(i&0x02){
			  // GPIO_SetBits(GPIOA,GPIO_Pin_4);
			}
			if(i&0x04){
				// GPIO_SetBits(GPIOA,GPIO_Pin_5);
			}
			// GPIO_SetBits(GPIOC,GPIO_Pin_4); //使能电机
			pmotor=&motor4;
			motor3.id=4;
			motor3.clockwise=M4_CLOCKWISE;
			motor3.TIMx=TIM3;
			MotorTimeTable=Motor3TimeTable;
			MotorStepTable=Motor3StepTable;	
	  default:
      break;
  }
	if(MotorID<=4&&MotorID>=1){
		pmotor->divnum=StepDive;
		pmotor->MaxPosition=maxposition;
		pmotor->MaxPosition_Pulse=maxposition*StepDive;

		pmotor->CurrentPosition=0;
		pmotor->CurrentPosition_Pulse=0;
		pmotor->StartTableLength=STEP_AA+STEP_UA+STEP_RA+1;
		pmotor->StopTableLength=STEP_AA+STEP_UA+STEP_RA; 
		pmotor->Counter_Table=MotorTimeTable;
		pmotor->Step_Table=MotorStepTable;

		pmotor->CurrentIndex=0;
		pmotor->speedenbale=0;
		pmotor->StartSteps=0;                  //必须清零，后面是累加，否则会把前一次的加上
		pmotor->StopSteps=0;                   //同上
		for(i=0;i<pmotor->StartTableLength;i++){
      pmotor->StartSteps+=pmotor->Step_Table[i];
    }
		for(i=0;i<pmotor->StopTableLength;i++){
      pmotor->StopSteps+=pmotor->Step_Table[i+pmotor->StartTableLength];
    }
		pmotor->TIMx->ARR=pmotor->Counter_Table[0];            //设置周期
		pmotor->TIMx->CCR1=pmotor->Counter_Table[0]>>1;        //设置占空比
		pmotor->TIMx->CCR2=pmotor->Counter_Table[0]>>1;        //设置占空比
	}
}

/*启动电机按照S型曲线参数运行*/
void Start_Motor_S(unsigned char MotorID,unsigned char dir,unsigned int Degree)
{
  unsigned int PulsesGiven=0;
	MOTOR_CONTROL_S *pmotor=NULL; 
	if(Degree==0){ 		  	 
		return;
	}
	switch(MotorID){
		case 1:
			pmotor=&motor1; 
			if(0==dir){     //设置电机旋转方向
		    // GPIO_SetBits(GPIOE,GPIO_Pin_9);
		  }else{
		    // GPIO_ResetBits(GPIOE,GPIO_Pin_9);
		  } 			
			break;
		case 2:
			pmotor=&motor2; 
		  if(1==dir){     //设置电机旋转方向
		    // GPIO_SetBits(GPIOA,GPIO_Pin_1);
		  }else{
		    // GPIO_ResetBits(GPIOA,GPIO_Pin_1);  
		  }	
			break;
		case 3:
			pmotor=&motor3; 
		  if(0==dir){   //设置电机旋转方向
		    // GPIO_SetBits(GPIOA,GPIO_Pin_7);
		  }else{
		    // GPIO_ResetBits(GPIOA,GPIO_Pin_7);
		  }	
			break;
    case 4:
      pmotor=&motor4;
      if(0==dir){   //设置电机旋转方向
		    // GPIO_SetBits(GPIOA,GPIO_Pin_7);
		  }else{
		    // GPIO_ResetBits(GPIOA,GPIO_Pin_7);
		  }	
			break;
		default:
			return;
	}
	pmotor->en=1;
	pmotor->dir=dir;
	pmotor->running=1;
	pmotor->PulsesHaven=0;
	PulsesGiven=Degree;
	pmotor->Time_Cost_Act=0;
	pmotor->PulsesGiven=PulsesGiven*pmotor->divnum;
	Motor_Reinitial(MotorID);		
	pmotor->CurrentIndex=0;
	pmotor->speedenbale=0;
	pmotor->TIMx->ARR =Motor1TimeTable[0];            //设置周期
	pmotor->TIMx->CCR1=Motor1TimeTable[0]>>1;        //设置占空比
	pmotor->TIMx->CCR2=Motor1TimeTable[0]>>1;        //设置占空比
	TIM_Cmd(pmotor->TIMx,ENABLE);		                //ENABLE
}

/*重新初始化电机运行时相关参数*/
void Motor_Reinitial(unsigned char MotorID)
{
	int i=0; 
	MOTOR_CONTROL_S *pmotor=NULL;  
	switch(MotorID){
		case 1:
			pmotor=&motor1;
			break;
		case 2:
			pmotor=&motor2;
			break;
		case 3:
			pmotor=&motor3;
			break;
    case 4:
			pmotor=&motor4;
			break;
		default:
			return ;
	}					 
	pmotor->pulsecount=0;
	pmotor->CurrentIndex=0;
	pmotor->speedenbale=0;
	pmotor->StartSteps=0;                  //必须清零，后面是累加，否则会把前一次的加上
	pmotor->StopSteps=0;                   //同上
	for(i=0;i<pmotor->StartTableLength;i++){
    pmotor->StartSteps+=pmotor->Step_Table[i];
  }
	for(i=0;i<pmotor->StopTableLength;i++){
    pmotor->StopSteps+=pmotor->Step_Table[i+pmotor->StartTableLength];
  }
	pmotor->TIMx->ARR=pmotor->Counter_Table[0];            //设置周期
	pmotor->TIMx->CCR1=pmotor->Counter_Table[0]>>1;        //设置占空比
	pmotor->TIMx->CCR2=pmotor->Counter_Table[0]>>1;        //设置占空比
	pmotor->Time_Cost_Act=pmotor->TIMx->ARR;
	Get_TimeCost_ReverDot_S(MotorID);		 
}

/*多轴协同使用了算法原理进行时间预估，所以修改该算法时记得
 这两处保持同步*/
/*计算S型曲线反转点，S型曲线在运行时，加减速过程是完全对称的*/
unsigned long long Get_TimeCost_ReverDot_S(unsigned char MotorID)
{
	unsigned long long time_cost=0;
	unsigned long long time_cost2=0;
	unsigned int pulsecnt=0;
	int i=0,j;
	MOTOR_CONTROL_S *pmotor=NULL; 
	switch(MotorID){
		case 1:
			pmotor=&motor1;
			break;
		case 2:
			pmotor=&motor2;
			break;
		case 3:
			pmotor=&motor3;
			break;
    case 4:
			pmotor=&motor4; 
			break;
		default:
			return 0;
	}
	if(pmotor->PulsesGiven>=pmotor->StartSteps+pmotor->StopSteps){
		for(i=0;i<pmotor->StartTableLength;i++){
      time_cost+=(pmotor->Step_Table[i]*pmotor->Counter_Table[i]);
    }
		for(i=0;i<pmotor->StopTableLength;i++){
      time_cost+=(pmotor->Step_Table[i+pmotor->StartTableLength]*pmotor->Counter_Table[i+pmotor->StartTableLength]);
    }		
		time_cost+=(pmotor->PulsesGiven-pmotor->StartSteps-pmotor->StopSteps)*pmotor->Counter_Table[pmotor->StartTableLength-1];
		pmotor->RevetDot=pmotor->PulsesGiven-pmotor->StopSteps;
	}else{
		//考虑这种情况，第一频率142 步，第二频率148步，要是运动200步该怎么运行
		//所以这里要改变第二频率的步数
		while((pulsecnt+pmotor->Step_Table[i])<=(pmotor->PulsesGiven>>1)){					
			time_cost+=(pmotor->Step_Table[i]*pmotor->Counter_Table[i]);
			time_cost2+=(pmotor->Step_Table[i]*pmotor->Counter_Table[i]);
			pulsecnt+=pmotor->Step_Table[i];
			i++;
		}
		time_cost+=time_cost2;
		if(pmotor->Step_Table[i]<pmotor->PulsesGiven-2*pulsecnt){
			pmotor->Step_Table[i]=pmotor->PulsesGiven-2*pulsecnt;
			pmotor->StartSteps=0;                  //必须清零，后面是累加，否则会把前一次的加上
			pmotor->StopSteps=0;                   //同上
			for(j=0;j<pmotor->StartTableLength;j++){
        pmotor->StartSteps+=pmotor->Step_Table[j];
      }
			for(j=0;j<pmotor->StopTableLength;j++){
        pmotor->StopSteps+=pmotor->Step_Table[j+pmotor->StartTableLength];
      }
		}
		time_cost+=(pmotor->Counter_Table[i]*(pmotor->PulsesGiven-2*pulsecnt));
		pmotor->RevetDot=pmotor->PulsesGiven-pulsecnt;
	}
	pmotor->Time_Cost_Cal=time_cost;
	return time_cost;
}

 /*电机S型曲线算法公共处理函数*/
void TIMX_UP_IRQHandler_S(MOTOR_CONTROL_S* pmotor)
{   
	if(1==pmotor->en){ 
		//位置计算
		if(pmotor->clockwise==pmotor->dir){
			pmotor->CurrentPosition_Pulse++;
			if(pmotor->CurrentPosition_Pulse>=pmotor->MaxPosition_Pulse){
				pmotor->CurrentPosition_Pulse=0;
			}
		}else{
			pmotor->CurrentPosition_Pulse--;
			if(pmotor->CurrentPosition_Pulse==0xffffffff){
				pmotor->CurrentPosition_Pulse=pmotor->MaxPosition_Pulse-1;
			}
		}
		pmotor->CurrentPosition=pmotor->CurrentPosition_Pulse/pmotor->divnum;
		//速度控制
		if(pmotor->speedenbale&&(pmotor->CurrentIndex==pmotor->TargetIndex||
		pmotor->TargetIndex+pmotor->CurrentIndex==pmotor->StartTableLength+pmotor->StopTableLength-1)){
			return;
		}
		pmotor->PulsesHaven++; //总脉冲个数
		pmotor->pulsecount++;  //以该频率脉冲输出的脉冲个数 
		//对称反转
		if(pmotor->RevetDot==pmotor->PulsesHaven){
			pmotor->pulsecount=pmotor->Step_Table[pmotor->CurrentIndex];
		}
		if(pmotor->pulsecount>=pmotor->Step_Table[pmotor->CurrentIndex]){ 
			if(pmotor->PulsesHaven<=pmotor->StartSteps){
				//起步阶段
				if(pmotor->CurrentIndex<pmotor->StartTableLength-1){
					pmotor->CurrentIndex++;
					pmotor->pulsecount=0;
					if(pmotor->CurrentIndex>=pmotor->StartTableLength)pmotor->CurrentIndex=pmotor->StartTableLength;
				}
			}
			//对于速度控制，此处不能判断pmotor->PulsesHaven>=(pmotor->PulsesGiven>>1)
			//if(pmotor->PulsesGiven-pmotor->PulsesHaven<=pmotor->StopSteps&&pmotor->PulsesHaven>=(pmotor->PulsesGiven>>1))
			if((pmotor->PulsesGiven-pmotor->PulsesHaven<=pmotor->StopSteps&&pmotor->speedenbale==1)||
				(pmotor->PulsesGiven-pmotor->PulsesHaven<=pmotor->StopSteps&&pmotor->speedenbale==0&&
				pmotor->PulsesHaven>=(pmotor->PulsesGiven>>1))) {
				//停止阶段
				if(pmotor->CurrentIndex<pmotor->StartTableLength-1){
					pmotor->CurrentIndex=pmotor->StartTableLength+pmotor->StopTableLength-pmotor->CurrentIndex;  
				}
				pmotor->CurrentIndex++;
				pmotor->pulsecount=0;
				if(pmotor->CurrentIndex>=pmotor->StartTableLength+pmotor->StopTableLength){
					pmotor->CurrentIndex=pmotor->StartTableLength+pmotor->StopTableLength-1;
				}
			}  		
			pmotor->TIMx->ARR=pmotor->Counter_Table[pmotor->CurrentIndex] ; //设置周期
			pmotor->TIMx->CCR1=( pmotor->Counter_Table[pmotor->CurrentIndex])>>1;       //设置占空比
			pmotor->TIMx->CCR2=( pmotor->Counter_Table[pmotor->CurrentIndex])>>1;	    
		}	  
		//旋转预定脉冲数，停止，running=0，可以进行下一次旋转
		if(pmotor->PulsesHaven>=pmotor->PulsesGiven&&pmotor->PulsesHaven>3){
			pmotor->en=0;
			pmotor->running=0;
			pmotor->CurrentIndex=0;
			TIM_Cmd(pmotor->TIMx,DISABLE);		  //DISABLE 			
		}else{			
			pmotor->Time_Cost_Act+=pmotor->TIMx->ARR;
		}
	}
}

/**
  * 函数功能: 第一象限圆弧顺时针运动
  * 输入参数: coordsA,coordsB:  分别是起点坐标相对于圆心坐标增量,单位是脉冲数
  *           Xe,Ye:  分别是终点相对于起点的坐标增量,单位是脉冲数
  *           Speed:  速度值
  * 返 回 值: 无
  * 说    明: 参数设置兼容G代码,即给定终点相对圆弧起点的坐标,和圆心相对于圆弧起点的坐标
  *           在第一象限做一个半径为5个单位(5*6400个脉冲)的1/4圆弧
  */
void Arc_IncMoveCW(int32_t Xe,int32_t Ye,int32_t coordsA,int32_t coordsB,uint32_t Speed)
{
  if(MotionStatus == 1)   // 当前电机正在运动 
    return ;
  
  /* 起点相对于圆心的坐标,圆心视为坐标轴的原点 */
	Rel_Position[AXIS_X]= 0 - coordsA;    // 当前位置就是起点
	Rel_Position[AXIS_Y]= 0 - coordsB;    // 将圆心坐标视为坐标轴的原点,得到当前位置相对于圆心的坐标
  
  /* 终点坐标 */
  Arc.END_X = Rel_Position[AXIS_X]+Xe; // 相对于圆心的终点坐标
	Arc.END_Y = Rel_Position[AXIS_Y]+Ye; // 相对于圆心的终点坐标

  /* 不符合圆的坐标方程就返回 */
  if( (pow(Rel_Position[AXIS_X],2)+pow(Rel_Position[AXIS_Y],2)) != (pow(Arc.END_X,2)+pow(Arc.END_Y,2)) )
    return ;
    
  /* 总的步数 */
  Arc.END_Y = iabs(Arc.END_Y - Rel_Position[AXIS_Y]);
  Arc.END_X = iabs(Arc.END_X - Rel_Position[AXIS_X]);
  Arc.END_Pulse = Arc.END_Y + Arc.END_X;  // 从起点到终点的脉冲数
  
  /* 第一象限的圆弧顺时针方向 */
  Arc.X_Dir = M1_CLOCKWISE;
  Arc.Y_Dir = M1_UNCLOCKWISE;
  // STEPMOTOR_DIR_FORWARD(AXIS_X);
  // STEPMOTOR_DIR_REVERSAL(AXIS_Y);
  
  Arc.F_e = 0;							            // 偏差方程置零
  /* 起点坐标x = 0,说明起点位于y轴上,此时往X轴进给误差会减少 */
  if(Rel_Position[AXIS_X] == 0)  
  {  
    Arc.Active_Axis = AXIS_X;    // 第一步给X轴
    Arc.F_e = Arc.F_e + 2*Rel_Position[AXIS_X] + 1;// 偏差方程的计算
  }
  else 
  {
    Arc.Active_Axis = AXIS_Y;    // 第一步给Y轴
    Arc.F_e = Arc.F_e - 2*Rel_Position[AXIS_Y] + 1;// 偏差方程的计算
  }
  Toggle_Pulse = Speed;   // 进给速度
  MotionStatus = 1;       // 标记电机正在运动

	TIM_SetCompare1(TIM3,Toggle_Pulse);
	TIM_SetCompare2(TIM3,Toggle_Pulse);
	TIM_SetAutoreload(TIM3,Toggle_Pulse*2);

	TIM_CCxCmd(TIM3,TIM_Channel_1,TIM_CCx_Enable);
	TIM_CCxCmd(TIM3,TIM_Channel_2,TIM_CCx_Enable);
  TIM_Cmd(TIM3,ENABLE);
}

/**
  * 函数功能: 定时器中断回调函数
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 实现加减速过程
  */
void TIMX_UP_IRQHandler_Arc(void)
{
  __IO uint32_t Axis = 0;	// 进给轴
  Axis = Arc.Active_Axis;   // 当前进给轴
	
  /* 判断是否到达终点或者还没开始运动 */
  if(Arc.END_Pulse == 0)
    return;
  /* 根据进给方向 更新坐标值 */
  if(Arc.Active_Axis == AXIS_X)
  {
    if(Arc.X_Dir == M1_UNCLOCKWISE)
    {  
      Step_Position[AXIS_X]--;	// 记录X轴绝对坐标位置
      Rel_Position[AXIS_X]--;
    }
    else 
    {
      Step_Position[AXIS_X]++;
      Rel_Position[AXIS_X]++;
    }
  }
  if(Arc.Active_Axis == AXIS_Y)
  {
    if(Arc.Y_Dir == M1_UNCLOCKWISE)
    {
      Step_Position[AXIS_Y]--;	// 记录Y轴绝对坐标位置
      Rel_Position[AXIS_Y]--;
    }
    else 
    {
      Step_Position[AXIS_Y]++;
      Rel_Position[AXIS_Y]++;
    }
  }
  
  /* 根据上一次的偏差判断下一步进给方向,同时计算下一次的偏差 */
  if(Arc.F_e >= 0)              // 偏差方程 > 0 ,说明当前位置位于圆弧外方,应向圆内进给
  {	
    Arc.Active_Axis = AXIS_Y;
    Arc.F_e = Arc.F_e - 2*Rel_Position[AXIS_Y] + 1;// 偏差方程的计算
    if(Axis != Arc.Active_Axis)       // 判断是否需要跟换进给轴
    {
			TIM_CCxCmd(TIM3,Stepmotor[Axis].Pulse_Channel,TIM_CCx_Disable);

			TIM_CCxCmd(TIM3,Stepmotor[Arc.Active_Axis].Pulse_Channel,TIM_CCx_Enable);
    }
  }
  else if(Arc.F_e < 0)         // 偏差方程 < 0 ,说明当前位置位于圆弧内侧,应向圆外进给
    {
      Arc.Active_Axis = AXIS_X;
      Arc.F_e = Arc.F_e + 2*Rel_Position[AXIS_X] + 1;// 偏差方程的计算
      if(Axis!=Arc.Active_Axis)
      {
				TIM_CCxCmd(TIM3,Stepmotor[Axis].Pulse_Channel,TIM_CCx_Disable);

				TIM_CCxCmd(TIM3,Stepmotor[Arc.Active_Axis].Pulse_Channel,TIM_CCx_Enable);
      }
    }
    
  /* 终点判别:总步长 */
  Arc.END_Pulse--;
  if( 0 == Arc.END_Pulse)
  {
    MotionStatus = 0;       // 到达终点
    TIM_Cmd(TIM3,DISABLE);	// 失能定时器
    return ;
  }
  else
  {
		if(Stepmotor[Arc.Active_Axis].Pulse_Channel==TIM_Channel_1){
			TIM_SetCompare1(TIM3,Toggle_Pulse);
		}else{
			TIM_SetCompare2(TIM3,Toggle_Pulse);
		}
		TIM_SetAutoreload(TIM3,Toggle_Pulse*2);
		TIM_Cmd(TIM3,ENABLE);
  }
}